import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster 
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf_transformations

class DynamictsTFBroadcasterPy(Node):
    def __init__(self):
        super().__init__("dynamic_tf_broadcaster_py_node")
        self.broadcaster=TransformBroadcaster(self)
        self.subscription=self.create_subscription(Pose,"/turtle1/pose", self.do_pose,10)
    def do_pose(self,pose):
        ts=TransformStamped()
        ts.header.stamp=self.get_clock().now().to_msg()
        ts.header.frame_id="world"
        ts.child_frame_id="turtle1"

        ts.transform.translation.x=pose.x
        ts.transform.translation.y=pose.y
        ts.transform.translation.z=0.0

        
        qtn=tf_transformations.quaternion_from_euler(0.0,0.0,pose.theta)

        ts.transform.rotation.x=qtn[0]
        ts.transform.rotation.y=qtn[1]
        ts.transform.rotation.z=qtn[2]
        ts.transform.rotation.w=qtn[3]
        
        self.broadcaster.sendTransform(ts)
 
def main():
    rclpy.init()
    rclpy.spin(DynamictsTFBroadcasterPy())
    rclpy.shutdown()
if __name__=='__main__':
    main()
